#include <stdlib.h>
#include "motion9_dll.h"
#include "position_out.h"
#include "c_mutex.h"
static int poqCheck(STRUCT_PO_QUEUE const * const tcq);

static int poqCheck(STRUCT_PO_QUEUE const * const tcq)
{
	if ((0 == tcq) || (0 == tcq->queue))
	{
		return -1;
	}
	return 0;
}

int poqCreate(STRUCT_PO_QUEUE * const tcq, int _size, STRUCT_PO_SEGMENT * const tcSpace)
{
	if (_size <= 0 || 0 == tcq) {
		return -1;
	}
	else {
		tcq->queue = tcSpace;
		tcq->size = _size;
		tcq->_len = 0;
		tcq->start = tcq->end = 0;
		tcq->allFull = 0;

		if (0 == tcq->queue) {
			return -1;
		}
		return 0;
	}
}

int poqDelete(STRUCT_PO_QUEUE * const tcq)
{
	if (!poqCheck(tcq)) {
		tcq->queue = 0;
	}

	return 0;
}

int poqInit(STRUCT_PO_QUEUE * const tcq)
{
	if (poqCheck(tcq)) return -1;
	tcq->_len = 0;
	tcq->start = tcq->end = 0;
	tcq->allFull = 0;
	return 0;
}

int poqPut(STRUCT_PO_QUEUE * const tcq, STRUCT_PO_SEGMENT const * const tc)
{
	if (poqCheck(tcq)) return -1;
	if (tcq->allFull) {
		return -1;
	}
	tcq->queue[tcq->end] = *tc;
	tcq->_len++;
	tcq->end = (tcq->end + 1) % tcq->size;
	if (tcq->end == tcq->start) {
		tcq->allFull = 1;
	}
	return 0;
}

int poqPopBack(STRUCT_PO_QUEUE * const tcq)
{
	if (poqCheck(tcq)) return -1;
	if (tcq->_len < 1) {
		return -1;
	}
	int n = tcq->end - 1 + tcq->size;
	tcq->end = n % tcq->size;
	tcq->_len--;
	return 0;
}

int poqRemove(STRUCT_PO_QUEUE * const tcq, int n)
{
	if (n <= 0) {
		return 0;
	}
	if (poqCheck(tcq) || ((tcq->start == tcq->end) && !tcq->allFull) ||
		(n > tcq->_len)) {	/* too many requested */
		return -1;
	}
	tcq->start = (tcq->start + n) % tcq->size;
	tcq->allFull = 0;
	tcq->_len -= n;
	return 0;
}

int poqLen(STRUCT_PO_QUEUE const * const tcq)
{
	if (poqCheck(tcq)) return -1;

	return tcq->_len;
}

STRUCT_PO_SEGMENT * poqItem(STRUCT_PO_QUEUE const * const tcq, int n)
{
	if (poqCheck(tcq) || (n < 0) || (n >= tcq->_len)) return NULL;
	return &(tcq->queue[(tcq->start + n) % tcq->size]);
}

int poqFull(STRUCT_PO_QUEUE const * const tcq)
{
	if (poqCheck(tcq)) {
		return 1;
	}
	return tcq->allFull;
	return 0;
}

STRUCT_PO_SEGMENT *poqLast(STRUCT_PO_QUEUE const * const tcq)
{
	if (poqCheck(tcq)) {
		return NULL;
	}
	if (tcq->_len == 0) {
		return NULL;
	}
	//Fix for negative modulus error
	int n = tcq->end - 1 + tcq->size;
	return &(tcq->queue[n % tcq->size]);
}


